from calc_cost import CalcCost
from ego_property import EgoProperty
from output import Output
from ref_point import RefPoint
from sample import *
from traj_filter import TrajFilter
from transformer import *
from merge_traj import *

import numpy as np
import matplotlib.pyplot as plt

class RefLine:
    def __init__(self,x_data,y_data):
        self.f=np.poly1d(np.polyfit(x_data,y_data,len(x_data)-1))
    def getRefLine(self):
        return self.f
# plot env
x=np.array([10,12,15,16])
y=np.array([0,45,90,100])

refline=RefLine(x,y)
x_data=np.linspace(9,16,100)
y_data=refline.getRefLine()(x_data)
print(refline.getRefLine())

plt.plot(x_data,y_data)
# plot obj
x=[11,15]
y=[45,80]
plt.scatter(x,y,c='r')
# plot ego
ego_pose=[9,1]
plt.scatter(ego_pose[0],ego_pose[1],c='g')

# start lattice
refpoint = RefPoint(refline,10,20)

while True:
    cloest_point=refpoint.getCloestPoint(ego_pose)
    #TODO add ego property: velocity, acceleration, theta and kappa
    ego_point=[]
    ego_trans2Fre = C2F(cloest_point,ego_point)
    ego_trans2Fre.run()
    origin=ego_trans2Fre.get()

    # long sample
    #TODO: 1. add ego property: velocity; 2. origin state
    end_v= {"v":[],"a":[]}
    a_max=2
    nums=6
    steptimes=[0.1,1,2,3,4,5,6,7,8]
    for t in steptimes:
        v_max=min(a_max*t+ego_v,10)
        v_min=max(-a_max*t+ego_v,0)
        end_v["v"].append(np.linspace(v_min,v_max,6))
        end_v["a"].append(np.linspace(0,0,6))
    longTraj = QuarticSample(origin,end_v,steptimes).run()
    # lateral sample
    #TODO: origin state
    end={"var":[],"var_dot":[],"var_2dot":[]}
    nums=3
    steps = [5,10,15,20]
    for t in steps:
        end["var"].append(np.array([-0.5,0,0.5]))
        end["var_dot"].append(np.array([0, 0, 0]))
        end["var_2dot"].append(np.array([0, 0, 0]))
    lateralTraj = QuinticSample(origin,end,steps).run()
    # filter
    traj_filter = TrajFilter(lateralTraj,longTraj)
    traj_filter.run()
    # calc cost
    cost_calc = CalcCost(lateralTraj,longTraj)
    cost_calc.run()
    bestTraj = cost_calc.getTop()
    # Merge Traj
    TrajMerger=Merge(bestTraj)
    # TODO: add tTable, refline_fun
    bestTrajPoint_1 = TrajMerger.run()
    F2C().run()

    Output.run()


# end